/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package labcontrole;

import br.ufrn.dca.controle.QuanserClient;
import br.ufrn.dca.controle.QuanserClientException;
import java.util.Timer;
import java.util.TimerTask;
import java.util.logging.Level;
import java.util.logging.Logger;
import org.apache.commons.math.complex.Complex;
import org.jfree.chart.ChartFactory;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.data.xy.XYSeries;
import org.jfree.data.xy.XYSeriesCollection;

/**
 * Essa classe é utilizada para realizar a leitura/escrita de dados na planta e
 * apresentar esses dados graficamente na interface, e serve ainda como uma
 * "ponte" entre a interface gráfica e a classe responsável pela implementação
 * do controlador1.
 * @author victor
 */
public class FThread {

    // Constantes
    private final double INC_TEMPO = 0.1;
    private final long T_AMOSTRAGEM = 100;
    private final double CTE_TANQUE = 6.25;
    private final double TENSAO_SATURACAO = 3;
    private final double NIVEL_MAX = 28;
    // Referencia para a interface grafica
    TelaPrincipal ui;
    // Controle de tempo
    private double tempo;
    // Graficos
    private XYSeriesCollection datasetEscrita, datasetLeitura;
    private JFreeChart chartEscrita, chartLeitura;
    private XYSeries sinalControlador1, sinalControlador2;
    private XYSeries sinalReferencia, sinalTanque1, sinalTanque2, sinalErro;
    private int numAmostras;
    // Interface com o controlador
    Controlador controlador1, controlador2;
    private String modoMalha;
    private double erroT1, erroT2;
    private double erroAnteriorT1, erroAnteriorT2;
    private Double nivelT1, nivelT2;
    private int referencia;
    private Double sinalControleT1, sinalControleT2;
    // Interface com a classe Sinal (malha aberta)
    private Sinal sinal;
    // Comunicacao com os tanques
    private QuanserClient quanserClient;
    private int portaT1, portaT2;
    private int portaEscrita;
    // Flag usada p/ controlar a execucao da thread
    private boolean threadAtivada;
    // VARIAVEIS UTILIZADAS PARA O CASO DO SEGUIDOR DE REFERENCIA
    private Complex p1, p2, p3;

    public void setP1(Complex p1) {
        this.p1 = p1;
    }

    public void setP2(Complex p2) {
        this.p2 = p2;
    }

    public void setP3(Complex p3) {
        this.p3 = p3;
    }
    //private double p1, p2, p3;
    private double k1, k21, k22;
    double v;

    public FThread(TelaPrincipal ui) {
        this.ui = ui;

        portaT1 = 0; // Porta padrao do tanque 1 (superior) eh a porta 0
        portaT2 = 1; // Porta padrao do tanque 2 (inferior) eh a porta 1
        portaEscrita = portaT1; // Soh eh possivel escrever no tanque 1

        erroAnteriorT1 = erroT1 = 0;
        erroAnteriorT2 = erroT2 = 0;
        controlador1 = new Controlador();
        controlador2 = new Controlador();

        k1 = k21 = k22 = 1;

        initGraficos();
        initTimer();
    }

    /**
     * Método para calcular os ganhos no modo de espaço de estados.
     */
    public void calcularGanhos() {
        Complex a12 = p1.multiply(p2);
        Complex a13 = p1.multiply(p3);
        Complex a23 = p2.multiply(p3);
        Complex a123 = p1.multiply(p2).multiply(p3);
        Complex b123 = p1.add(p2).add(p3);

        //k1 = 3277.5 * (p1*p2 - p1 - p2 - p3 + p1*p3 + p2*p3 - p1*p2*p3 + 1);
        Complex C0 = new Complex(3277.5, 0);
        k1 = C0.multiply(a12.subtract(b123).add(a13).add(a23).subtract(a123).add(new Complex(1, 0))).getReal();

        //k21 = 36.3958 - 5.2216*(p1+p2+p3) - 5.2439*(p1*p2 + p1*p3 + p2*p3) - 5.2664*p1*p2*p3;
        Complex C1 = new Complex(36.3958, 0);
        Complex C2 = new Complex(5.2216, 0);
        Complex C3 = new Complex(5.2439, 0);
        Complex C4 = new Complex(5.2664, 0);
        k21 = C1.subtract(C2.multiply(b123)).subtract(C3.multiply(a12.add(a13.add(a23)))).subtract(C4.multiply(a123)).getReal();

        //k22 = 4876.2 + 4925.0*p1*p2*p3 - 1630.0*(p1+p2+p3) - 1637.0*(p1*p2 + p1*p3 + p2*p3);
        Complex D1 = new Complex(4876.2, 0);
        Complex D2 = new Complex(4925.0, 0);
        Complex D3 = new Complex(1630.0, 0);
        Complex D4 = new Complex(1637.0, 0);
        k22 = D1.add(D2.multiply(a123)).subtract(D3.multiply(b123)).subtract(D4.multiply(a12.add(a13).add(a23))).getReal();

        System.out.print("k1: ");
        System.out.println(k1);
        System.out.print("k21: ");
        System.out.println(k21);
        System.out.print("k22: ");
        System.out.println(k22);

        ui.setGanhoK1(k1);
        ui.setGanhoK21(k21);
        ui.setGanhoK22(k22);

    }

    private void initTimer() {
        TimerTask task = new TimerTask() {

            public void run() {
                fRun();
            }
        };
        Timer timer = new Timer();
        timer.schedule(task, 0, T_AMOSTRAGEM);
    }

    private void initGraficos() {
        numAmostras = 1500;

        sinalControlador1 = new XYSeries("Sinal de controle 1");
        sinalControlador2 = new XYSeries("Sinal de controle 2");
        sinalControlador1.setMaximumItemCount(numAmostras);
        sinalControlador2.setMaximumItemCount(numAmostras);

        datasetEscrita = new XYSeriesCollection();
        datasetEscrita.addSeries(sinalControlador1);
        datasetEscrita.addSeries(sinalControlador2);

        chartEscrita = ChartFactory.createXYLineChart(
                null, // chart title
                "Tempo (seg)", // x axis label
                "Tensão (v)", // y axis label
                datasetEscrita, // data
                PlotOrientation.VERTICAL,
                false, // include legend
                true, // tooltips
                false // urls
                );


        sinalReferencia = new XYSeries("Referência");
        sinalTanque1 = new XYSeries("Tanque 1");
        sinalTanque2 = new XYSeries("Tanque 2");
        sinalErro = new XYSeries("Erro");
        sinalReferencia.setMaximumItemCount(numAmostras);
        sinalTanque1.setMaximumItemCount(numAmostras);
        sinalTanque2.setMaximumItemCount(numAmostras);
        sinalErro = new XYSeries("Erro");

        datasetLeitura = new XYSeriesCollection();
        datasetLeitura.addSeries(sinalReferencia);
        datasetLeitura.addSeries(sinalTanque1);
        datasetLeitura.addSeries(sinalTanque2);
        datasetLeitura.addSeries(sinalErro);

        chartLeitura = ChartFactory.createXYLineChart(
                null, // chart title
                "Tempo (seg)", // x axis label
                "Altura (cm)", // y axis label
                datasetLeitura, // data
                PlotOrientation.VERTICAL,
                true, // include legend
                true, // tooltips
                false // urls
                );
    }

    /**
     * Metodo que realiza a conexao com o servidor (simulador ou planta real)
     * @param ip
     * @param porta
     * @return
     * @throws Exception
     */
    public boolean conectar(String ip, int porta) throws Exception {
        try {
            quanserClient = new QuanserClient(ip, porta);
            return true;
        } catch (Exception ex) {
            throw ex;
        }
    }

    public void setSinal(String tipo, double amplitude, double periodo) {
        if (sinal == null) {
            sinal = new Sinal();
        }
        sinal.setTipo(tipo);
        sinal.setAmplitude(amplitude);
        sinal.setPeriodo(periodo);
    }

    public void setTipoControlador(String string) {
        controlador1.setTipo(string);
    }

    public void setTipoControlador(String string1, String string2) {
        controlador1.setTipo(string1);
        controlador2.setTipo(string2);
    }

    public void plot() {
        // Reiniciar os graficos
        resetGraficos();
        // Habilitar execucao da thread
        threadAtivada = true;
    }

    private void resetGraficos() {
        tempo = 0;
        sinalControlador1.clear();
        sinalControlador2.clear();
        sinalReferencia.clear();
        sinalTanque1.clear();
        sinalTanque2.clear();
        sinalErro.clear();
    }

    /**
     * Desabilita a execucao da thread responsavel pelo acionamento da planta
     */
    public void parar() {
        try {
            quanserClient.write(portaEscrita, 0);
        } catch (QuanserClientException ex) {
            Logger.getLogger(FThread.class.getName()).log(Level.SEVERE, null, ex);
        }
        threadAtivada = false;
    }

    public JFreeChart getChartEscrita() {
        return chartEscrita;
    }

    public JFreeChart getChartLeitura() {
        return chartLeitura;
    }

    public void setModoMalha(String modoMalha) {
        this.modoMalha = modoMalha;
    }

    public void setReferencia1(int value) {
        referencia = value;
        controlador1.setReferenciaAnterior(controlador1.getReferencia());
        controlador1.setReferencia(value);
    }

    public void setReferencia2(int value) {
        referencia = value;
        controlador2.setReferenciaAnterior(controlador2.getReferencia());
        controlador2.setReferencia(value);
    }

    public void setKd(Double Kd) {
        controlador1.setKd(Kd);
    }

    public void setKi(Double Ki) {
        controlador1.setKi(Ki);
    }

    public void setKp(Double Kp) {
        controlador1.setKp(Kp);
    }

    public void setKd1(Double Kd1) {
        controlador1.setKd(Kd1);
    }

    public void setKi1(Double Ki1) {
        controlador1.setKi(Ki1);
    }

    public void setKp1(Double Kp1) {
        controlador1.setKp(Kp1);
    }

    public void setKd2(Double Kd2) {
        controlador2.setKd(Kd2);
    }

    public void setKi2(Double Ki2) {
        controlador2.setKi(Ki2);
    }

    public void setKp2(Double Kp2) {
        controlador2.setKp(Kp2);
    }

    public double getK1() {
        return k1;
    }

    public void setK1(double k1) {
        this.k1 = k1;
    }

    public double getK21() {
        return k21;
    }

    public void setK21(double k21) {
        this.k21 = k21;
    }

    public double getK22() {
        return k22;
    }

    public void setK22(double k22) {
        this.k22 = k22;
    }

    public void zerarErro() {
        erroAnteriorT1 = erroT1 = 0;
        erroAnteriorT2 = erroT2 = 0;
    }

    /**
     * Limitar sinal entre -3 e 3 (limitacao de tensao da planta)
     */
    private void saturarSinal() {
        if (sinalControleT1 > TENSAO_SATURACAO) {
            sinalControleT1 = TENSAO_SATURACAO;
        } else if (sinalControleT1 < -TENSAO_SATURACAO) {
            sinalControleT1 = -TENSAO_SATURACAO;
        }
    }

    /**
     * Implementa uma trava de seguranca que evita que o tanque transborde
     * @todo Deixar a trava mais robusta
     * @param nivel
     */
    private void limitarNivelT1() {
        // Trava superior
        if (nivelT1 >= NIVEL_MAX - 2 && sinalControleT1 > 0.0) {
            if (nivelT1 >= NIVEL_MAX - 1 && nivelT1 < NIVEL_MAX) {
                sinalControleT1 = 1.82;
            } else if (nivelT1 >= NIVEL_MAX){
                sinalControleT1 = 0.0;
            }
        }
        // Trava inferior
        if ((nivelT1) <= 6.0 && sinalControleT1 < 0.0) {
            sinalControleT1 = 0.0;
        }
    }

    /**
     * @todo Falta funcionar
     */
    public void esvaziar() {
        try {
            sinalControleT1 = controlador1.esvaziar(nivelT1);
            quanserClient.write(portaT1, sinalControleT1);
        } catch (QuanserClientException ex) {
            Logger.getLogger(FThread.class.getName()).log(Level.SEVERE, null, ex);
        }
    }
    
    public void calcularTempos(Controlador C, Double nivel){
        ui.setTempoAcomod(String.format("%.2f", C.getTempoAcomod(nivel)));
        //Condição para executar o calculo do tempo de Subida
        if (!C.getFlagSubida()) {
            ui.setTempoSubida(String.format("%.2f", C.getTempoSubida(nivel)));
        }
        //Condição para executar o calculo do tempo de Pico
        if (!C.getAchouOvershoot()) {
            ui.setTempoPico(String.format("%.2f", C.getTempoPico(nivel)));
        }
        //Condição para executar o calculo do Overshoot
        if (!C.getAchouOvershoot()) {
            ui.setOvershoot(String.format("%.2f", Math.abs(((C.getOverShoot(nivel)-C.getReferencia()))/C.getReferencia())*100));
        }
    }

    
    private void fRun() {
        try {
            if (threadAtivada) {
                controlador1.setTempo(tempo);
                controlador2.setTempo(tempo);
                nivelT1 = quanserClient.read(portaT1) * CTE_TANQUE;
                nivelT2 = quanserClient.read(portaT2) * CTE_TANQUE;
                controlador1.setNivelT1(nivelT1);
                controlador1.setNivelT2(nivelT2);
                controlador2.setNivelT1(nivelT1);
                controlador2.setNivelT2(nivelT2);

                if (modoMalha.equals("1aordem")) {
                    erroT1 = referencia - nivelT1;
                    controlador1.setInfo(erroT1, erroAnteriorT1, nivelT1);
                    erroAnteriorT1 = erroT1;
                    sinalControleT1 = controlador1.getSinalControle();

                    if (ui.isGraficoErroSelected()) {
                        sinalErro.add(tempo, erroT1);
                    }
                    calcularTempos(controlador1, nivelT1);

                } else if (modoMalha.equals("2aordem")) {
                    erroT2 = referencia - nivelT2;
                    controlador1.setInfo(erroT2, erroAnteriorT2, nivelT2);
                    erroAnteriorT2 = erroT2;
                    sinalControleT1 = controlador1.getSinalControle();

                    if (ui.isGraficoErroSelected()) {
                        sinalErro.add(tempo, erroT2);
                    }
                    calcularTempos(controlador2, nivelT2);
                } else if (modoMalha.equals("cascata")) {
                    // Controlador Master
                    erroT2 = referencia - nivelT2;
                    controlador2.setInfo(erroT2, erroAnteriorT2, nivelT2);
                    erroAnteriorT2 = erroT2;
                    sinalControleT2 = controlador2.getSinalControle();
                    // Controlador Slave
                    erroT1 = sinalControleT2 - nivelT1;
                    controlador1.setInfo(erroT1, erroAnteriorT1, nivelT1);
                    erroAnteriorT1 = erroT1;
                    sinalControleT1 = controlador1.getSinalControle();

                    if (ui.isGraficoErroSelected()) {
                        sinalErro.add(tempo, erroT2);
                    }
                    calcularTempos(controlador2, nivelT2);
                } else if (modoMalha.equals("modoMalhaAberta")) {
                    sinalControleT1 = sinal.getValor(tempo);
                    calcularTempos(controlador1, nivelT1);
                } else if (modoMalha.equals("EE")) {
                    erroT2 = referencia - nivelT2;
                    v = erroT2 + erroAnteriorT2;
                    erroAnteriorT2 += erroT2;
                    sinalControleT1 = v * k1 - (k21 * nivelT1) - (k22 * nivelT2);

                    if (ui.isGraficoErroSelected()) {
                        sinalErro.add(tempo, erroT2);
                    }
                    calcularTempos(controlador2, nivelT2);
                }

                limitarNivelT1();
                saturarSinal();
                quanserClient.write(portaEscrita, sinalControleT1);

                if (ui.isGraficoRefSelected()) {
                    sinalReferencia.add(tempo, referencia);
                }
                if (ui.isGraficoControle1Selected()) {
                    sinalControlador1.add(tempo, sinalControleT1);
                }
                if (ui.isGraficoNivelT1Selected()) {
                    sinalTanque1.add(tempo, nivelT1);
                }
                if (ui.isGraficoNivelT2Selected()) {
                    sinalTanque2.add(tempo, nivelT2);
                }

                ui.setNivelT1(String.format("%.2f", nivelT1));
                ui.setNivelT2(String.format("%.2f", nivelT2));
                ui.setTensao(String.format("%.2f", sinalControleT1));

                tempo += INC_TEMPO;
            }
        } catch (Exception e) {
            //System.out.println("Falha na comunicação com os tanques");
        }
    }
}
